#include "gnss.h"

// 所谓漂移，是指定位状态下的静态漂移和移动漂移。非定位不认为是漂移
// 理论：速度阈值/加速度阈值/方向角阈值(后面引入)
// 速度阈值：一般车辆速度不会超过150km/h
// 加速度阈值：百公里加速按照5s来算，a = (100000/3600)/5加速度为5.56
// 方向角阈值：连续两个车速在高于30km/h, 情况下方向角变化不会超过45°~60°
// GPS车速 1S输出一条

// GPSDrifting

#define GPSDriftingNUM       10 // 漂移数据点

typedef struct _GPSData_t
{
    uint8_t  Islocated;
    uint16_t Speed;     // 速度 0.1km/h
    uint16_t Direction; // 方向 0~359，正北为0，顺时针
    uint32_t Latitude;  // 纬度 以度为单位的纬度值乘以 10 的 6 次方，精确到百万分之一度
    uint32_t Longitude; // 经度 以度为单位的经度值乘以 10 的 6 次方，精确到百万分之一度 
} GPSData_t;

typedef struct _GPSDataNode_t
{
    uint16_t Speed;        // 速度
    uint16_t Acceleration; // 相对于上一秒的加速度
    uint16_t  Direction;    // 当前角度
} GPSDataNode_t;

typedef struct _GPSSpeedFilter_t
{
    uint32_t       referLat;   // 纬度 以度为单位的纬度值乘以 10 的 6 次方，精确到百万分之一度
    uint32_t       referLot;   // 经度 以度为单位的经度值乘以 10 的 6 次方，精确到百万分之一度.
    uint16_t       referSpeed; // 有效参考速度
    uint8_t        referAngle; // 有效参考角度
    uint8_t        buffIndex;
    GPSDataNode_t  node[30];   // 连续漂移30秒，GPS模块都有问题了
} GPSSpeedFilter_t;

typedef struct
{
    uint8_t          FilterState; // 过滤器状态
    GPSSpeedFilter_t Filter;
} GPSDriftingContext_t;

static GPSDriftingContext_t gpsDrifting = {0};

enum
{
    kGNSSSPeedIdle,
    kGNSSSPeedNormal, // 空闲判断状态
    kGNSSSPeedAbnormal
};


void GpsDriftingInit(void)
{
    gpsDrifting.Filter.buffIndex  = 0;
    gpsDrifting.Filter.referSpeed = 0;
    gpsDrifting.Filter.referAngle = 0;
    gpsDrifting.Filter.referLat   = 0; // 实际应该读取上次有效值
    gpsDrifting.Filter.referLot   = 0; // 实际应该读取上次有效值
}

void GpsDriftingFilter(GPSData_t *Data)
{
    


}
